package com.hongji.lms5xxcommunicator.opc.trigger;

import com.hongji.lms5xxcommunicator.entity.ScanPointEntity;
import com.hongji.lms5xxcommunicator.grid.region.RegionRect;
import com.hongji.lms5xxcommunicator.mapper.ScanPointDataMapper;
import com.hongji.lms5xxcommunicator.opc.util.OpcIoHelper;
import lombok.extern.slf4j.Slf4j;

import java.util.*;
import java.util.concurrent.ExecutorService;
import java.util.concurrent.Executors;
import java.util.concurrent.atomic.AtomicBoolean;

import static java.lang.Double.isFinite;

@Slf4j
public class TruckBedDetectorService {

    private final OpcIoHelper io;
    private final ScanPointDataMapper mapper;

    // 区域节点
    private final String regionXMinNode, regionXMaxNode, regionYMinNode, regionYMaxNode;

    // 通用结果节点
    private final String resultModeNode, resultXNode, resultYNode, resultZNode;

    // 单位与限流
    private final double regionUnitScale; // PLC→计算
    private final double dbUnitScale;     // DB→计算
    private final int    maxPoints;

    // 平面/OBB 参数（“计算单位”，与 dbUnitScale 之后一致）
    private final double zBinSize;
    private final double zTol;
    private final double excludeBottomPct;
    private final int    minPlanePoints;
    private final boolean fitTiltPlane;

    // 分段参数（与点同单位）
    private final double segMaxLenM;
    private final String centerReqNodeId;
    private final int    pollMs;
    private final int    maxWaitMsPerCenter;

    // 执行器：避免阻塞 Milo 回调线程
    private final ExecutorService worker = Executors.newSingleThreadExecutor(r -> {
        Thread t = new Thread(r, "TruckBed-worker");
        t.setDaemon(true);
        return t;
    });
    private final AtomicBoolean running = new AtomicBoolean(false);

    public TruckBedDetectorService(OpcIoHelper io, ScanPointDataMapper mapper,
                                   String regionXMinNode, String regionXMaxNode,
                                   String regionYMinNode, String regionYMaxNode,
                                   String resultModeNode, String resultXNode, String resultYNode, String resultZNode,
                                   double regionUnitScale, double dbUnitScale, int maxPoints,
                                   double zBinSize, double zTol, double excludeBottomPct, int minPlanePoints, boolean fitTiltPlane,
                                   double segMaxLenM, String centerReqNodeId, int pollMs, int maxWaitMsPerCenter) {
        this.io = io; this.mapper = mapper;
        this.regionXMinNode = regionXMinNode; this.regionXMaxNode = regionXMaxNode;
        this.regionYMinNode = regionYMinNode; this.regionYMaxNode = regionYMaxNode;
        this.resultModeNode = resultModeNode; this.resultXNode = resultXNode; this.resultYNode = resultYNode; this.resultZNode = resultZNode;
        this.regionUnitScale = regionUnitScale; this.dbUnitScale = dbUnitScale; this.maxPoints = maxPoints;
        this.zBinSize = Math.max(1e-6, zBinSize);
        this.zTol = Math.max(1e-6, zTol);
        this.excludeBottomPct = Math.min(Math.max(excludeBottomPct, 0.0), 0.95);
        this.minPlanePoints = Math.max(1, minPlanePoints);
        this.fitTiltPlane = fitTiltPlane;
        this.segMaxLenM = Math.max(0.01, segMaxLenM);
        this.centerReqNodeId = centerReqNodeId;
        this.pollMs = Math.max(10, pollMs);
        this.maxWaitMsPerCenter = Math.max(this.pollMs, maxWaitMsPerCenter);
    }

    /** 外部入口：触发 code==3 时调用 */
    public void onTriggerTruckBed() {
        if (!running.compareAndSet(false, true)) {
            log.warn("TruckBedDetector 正在运行，忽略本次触发。");
            return;
        }
        log.info("【车斗识别】收到触发(code=3)。参数摘要：zBinSize={}，zTol={}，excludeBottomPct={}，minPlanePoints={}，fitTiltPlane={}，segMaxLen={}，pollMs={}，maxWaitMsPerCenter={}，dbUnitScale={}，regionUnitScale={}",
                zBinSize, zTol, excludeBottomPct, minPlanePoints, fitTiltPlane, segMaxLenM, pollMs, maxWaitMsPerCenter, dbUnitScale, regionUnitScale);

        worker.submit(() -> {
            long t0 = System.currentTimeMillis();
            try {
                computeAndEmitCenters();
            } catch (Throwable t) {
                log.error("【车斗识别】流程异常：{}", t.getMessage(), t);
                safeWriteMode(0);
            } finally {
                running.set(false);
                log.info("【车斗识别】完成，总耗时 {} ms。", (System.currentTimeMillis() - t0));
            }
        });
    }

    // ===== 主流程 =====
    private void computeAndEmitCenters() {
        // 立即置 mode=3
        safeWriteMode(3);

        RegionRect region = readRegionFromPlc();
        if (region == null) { finishToIdle("PLC 区域无效"); return; }
        log.info("【车斗识别】使用区域(计算单位)：x[{}, {}], y[{}, {}]", fmt(region.getXMin()), fmt(region.getXMax()), fmt(region.getYMin()), fmt(region.getYMax()));

        List<ScanPointEntity> rows = fetchPointsByRegion(region, maxPoints);
        log.info("【车斗识别】DB 返回点数：{}（limit={}，dbUnitScale={}）", rows.size(), maxPoints, dbUnitScale);
        if (rows.isEmpty()) { finishToIdle("DB 区域无点"); return; }

        // 直方图选主平面
        Stats st = new Stats();
        for (ScanPointEntity r : rows) {
            if (r==null || r.getZ()==null) continue;
            double z = r.getZ() * dbUnitScale;
            if (!isFinite(z)) continue;
            st.minZ = Math.min(st.minZ, z);
            st.maxZ = Math.max(st.maxZ, z);
            st.zs.add(z);
        }
        if (st.zs.isEmpty()) { finishToIdle("Z 为空"); return; }
        log.info("【直方图】Z范围：min={}，max={}（计算单位）；样本={} 点", fmt(st.minZ), fmt(st.maxZ), st.zs.size());

        double groundCut = st.minZ + (st.maxZ - st.minZ) * excludeBottomPct;
        log.info("【直方图】地面截断阈值 groundCut={}（按 excludeBottomPct={} 计算）", fmt(groundCut), excludeBottomPct);

        Map<Long,Integer> hist = new HashMap<>();
        for (double z : st.zs) {
            if (z < groundCut) continue;
            long b = (long)Math.floor(z / zBinSize);
            hist.put(b, hist.getOrDefault(b, 0) + 1);
        }
        log.info("【直方图】有效 bin 数={}（bin 宽={}）", hist.size(), zBinSize);
        if (hist.isEmpty()) { finishToIdle("直方图为空（疑似全为地面）"); return; }

        long bestBin = Long.MIN_VALUE; int bestCnt = 0;
        for (Map.Entry<Long,Integer> e : hist.entrySet()) {
            if (e.getValue() > bestCnt) { bestCnt = e.getValue(); bestBin = e.getKey(); }
        }
        double zCenter = (bestBin + 0.5) * zBinSize;
        log.info("【直方图】主平面 bin={}，binCount={}，zCenter={}", bestBin, bestCnt, fmt(zCenter));

        if (bestCnt < minPlanePoints) {
            log.info("【直方图】主平面点数不足：{}（阈值={}）。注意：这里是“单bin计数”，可能过严。", bestCnt, minPlanePoints);
            // 不中断，让邻域厚度 ±zTol 再做一次真实统计
        }

        // 邻域点（XY+Z），用于 PCA/OBB 与拟合
        List<double[]> pts = new ArrayList<>(bestCnt);
        for (ScanPointEntity r : rows) {
            Double xr=r.getX(), yr=r.getY(), zr=r.getZ();
            if (xr==null || yr==null || zr==null) continue;
            double x=xr*dbUnitScale, y=yr*dbUnitScale, z=zr*dbUnitScale;
            if (Math.abs(z - zCenter) <= zTol) pts.add(new double[]{x,y,z});
        }
        log.info("【邻域】|z - zCenter| ≤ {} 过滤后得到点数：{}", zTol, pts.size());
        if (pts.size() < minPlanePoints) { finishToIdle("邻域不足：" + pts.size()); return; }

        // PCA & OBB
        PCA pca = PCA.onXY(pts);
        double yawRad = Math.atan2(pca.ev1y, pca.ev1x);
        double yawDeg = Math.toDegrees(yawRad);
        log.info("【PCA】质心：({},{})，第一主方向：(ev1x={}, ev1y={})，yaw={}°",
                fmt(pca.cx), fmt(pca.cy), fmt(pca.ev1x), fmt(pca.ev1y), fmt(yawDeg));

        OBB obb = OBB.fromPoints(pts, pca);
        double L = obb.uMax - obb.uMin; // 计算单位
        log.info("【OBB】u区间=[{}, {}]，v中值={}，长度L={}", fmt(obb.uMin), fmt(obb.uMax), fmt(obb.vc), fmt(L));

        // 平面 z = a x + b y + c（可选）
        double[] plane = fitTiltPlane ? fitPlane(pts) : null;
        double medianZ = medianZ(pts);
        if (plane != null) {
            log.info("【拟合平面】z = a*x + b*y + c，其中 a={}，b={}，c={}", fmt(plane[0]), fmt(plane[1]), fmt(plane[2]));
        } else {
            log.info("【拟合平面】关闭，使用 Z 的中位数：{}", fmt(medianZ));
        }

        // 按第一主轴长度 L 均分（最大段长 segMaxLenM）
        int segN = Math.max(1, (int)Math.ceil(L / segMaxLenM));
        double segW = L / segN;
        log.info("【分段】最大段长={}，计算得到段数={}，段宽={}", fmt(segMaxLenM), segN, fmt(segW));

        // 预计算每段中心点（世界系/计算单位）
        List<double[]> centers = new ArrayList<>(segN);
        for (int i=0;i<segN;i++){
            double u = obb.uMin + (i + 0.5) * segW; // 中心
            double v = obb.vc;                      // 走中轴线
            double x = pca.cx + u*pca.ev1x + v*(-pca.ev1y);
            double y = pca.cy + u*pca.ev1y + v*( pca.ev1x);
            double z = (plane!=null) ? plane[0]*x + plane[1]*y + plane[2] : medianZ;
            centers.add(new double[]{x,y,z});
        }
        if (!centers.isEmpty()) {
            double[] c0 = centers.get(0);
            double[] cE = centers.get(centers.size()-1);
            log.info("【分段】示例中心点：第1段({},{},{})  ->  最后一段({},{},{})（均为计算单位）",
                    fmt(c0[0]), fmt(c0[1]), fmt(c0[2]), fmt(cE[0]), fmt(cE[1]), fmt(cE[2]));
        }

        // 循环读取 centerReqNode：值变化且 != 0 才发送下一段
        emitCentersByRequest(centers);

        // 收尾
        safeWriteMode(0);
        log.info("【车斗识别】分段中心发送完成，切回 mode=0。");
    }

    // ===== 循环输出逻辑 =====
    private void emitCentersByRequest(List<double[]> centers) {
        double last = readReqValueSafe();
        log.info("【分段应答】开始轮询节点 {}，初始值={}（变化且!=0则发送下一段）", centerReqNodeId, fmt(last));

        for (int i=0;i<centers.size();i++){
            long startWait = System.currentTimeMillis();
            long deadline = startWait + maxWaitMsPerCenter;

            while (System.currentTimeMillis() < deadline) {
                double cur = readReqValueSafe();
                if (cur != 0.0 && !nearlyEqual(cur, last)) {
                    double[] c = centers.get(i);
                    writeXYZToResult(c[0], c[1], c[2]);           // 写中心点
                    safeWriteMode(31 + i);                        // 31/32/33...
                    log.info("【分段应答】检测到请求值变化：{} -> {}，发送第{}段中心 (x={}, y={}, z={})，mode={}",
                            fmt(last), fmt(cur), (i+1), fmt(c[0]), fmt(c[1]), fmt(c[2]), (31+i));
                    last = cur;
                    break;
                }
                sleepMs(pollMs);
            }

            if (System.currentTimeMillis() >= deadline) {
                log.warn("【分段应答】等待第{}段超时({} ms)，自动跳过。centerReqNode 当前值={}，上次值={}",
                        (i+1), maxWaitMsPerCenter, fmt(readReqValueSafe()), fmt(last));
            }
        }
    }

    // ===== OPC / DB / 工具 =====
    private RegionRect readRegionFromPlc() {
        try {
            double xminPlc = io.readDoubleFlexible(regionXMinNode);
            double xmaxPlc = io.readDoubleFlexible(regionXMaxNode);
            double yminPlc = io.readDoubleFlexible(regionYMinNode);
            double ymaxPlc = io.readDoubleFlexible(regionYMaxNode);

            log.info("【PLC读取】区域节点：xMin({})={}, xMax({})={}, yMin({})={}, yMax({})={}",
                    regionXMinNode, fmt(xminPlc), regionXMaxNode, fmt(xmaxPlc),
                    regionYMinNode, fmt(yminPlc), regionYMaxNode, fmt(ymaxPlc));

            double xmin = xminPlc * regionUnitScale;
            double xmax = xmaxPlc * regionUnitScale;
            double ymin = yminPlc * regionUnitScale;
            double ymax = ymaxPlc * regionUnitScale;

            log.info("【PLC读取】区域已按 regionUnitScale={} 换算到计算单位：x[{}, {}], y[{}, {}]",
                    regionUnitScale, fmt(xmin), fmt(xmax), fmt(ymin), fmt(ymax));

            if (xmax < xmin) { double t=xmin; xmin=xmax; xmax=t; }
            if (ymax < ymin) { double t=ymin; ymin=ymax; ymax=t; }
            return new RegionRect(xmin, xmax, ymin, ymax);
        } catch (Exception e) {
            log.error("【PLC读取】读取区域失败：{}", e.getMessage(), e);
            return null;
        }
    }

    private List<ScanPointEntity> fetchPointsByRegion(RegionRect r, int limit) {
        try {
            List<ScanPointEntity> list = mapper.selectByRegion(
                    r.getXMin() / dbUnitScale, r.getXMax() / dbUnitScale,
                    r.getYMin() / dbUnitScale, r.getYMax() / dbUnitScale,
                    limit
            );
            log.info("【DB查询】selectByRegion(x:[{},{}], y:[{},{}], limit={})，传入 DB 的单位已按 dbUnitScale={} 还原。",
                    fmt(r.getXMin()/dbUnitScale), fmt(r.getXMax()/dbUnitScale),
                    fmt(r.getYMin()/dbUnitScale), fmt(r.getYMax()/dbUnitScale),
                    limit, dbUnitScale);
            return list;
        } catch (Exception e) {
            log.error("【DB查询】失败：{}", e.getMessage(), e);
            return Collections.emptyList();
        }
    }

    private void writeXYZToResult(double x, double y, double z) {
        double plcX = x / regionUnitScale;
        double plcY = y / regionUnitScale;
        double plcZ = z / regionUnitScale;

        log.info("【PLC写入】即将写入结果节点：X({})={}, Y({})={}, Z({})={}（PLC单位）；对应计算单位：x={}, y={}, z={}",
                resultXNode, fmt(plcX), resultYNode, fmt(plcY), resultZNode, fmt(plcZ),
                fmt(x), fmt(y), fmt(z));

        io.writeIfPresent(resultXNode, plcX);
        io.writeIfPresent(resultYNode, plcY);
        io.writeIfPresent(resultZNode, plcZ);
    }

    private void finishToIdle(String reason) {
        log.info("【车斗识别】结束：{}，切回 mode=0。", reason);
        safeWriteMode(0);
    }

    private void safeWriteMode(int mode) {
        log.info("【PLC写入】设置 mode({}) = {}", resultModeNode, mode);
        try { io.writeIfPresent(resultModeNode, mode); } catch (Exception e) {
            log.error("【PLC写入】写 mode 失败：{}", e.getMessage(), e);
        }
    }

    private double readReqValueSafe() {
        try {
            double v = io.readDoubleFlexible(centerReqNodeId);
            return isFinite(v) ? v : 0.0;
        } catch (Exception e) {
            // 这里不打 error，避免高频刷屏
            return 0.0;
        }
    }

    private static boolean nearlyEqual(double a, double b){ return Math.abs(a-b) <= 1e-9; }
    private static void sleepMs(long ms){ try { Thread.sleep(ms); } catch (InterruptedException ignored) {} }
    private static String fmt(double v){ return String.format(Locale.ROOT, "%.3f", v); }

    // ===== 数学与结构 =====
    private static class Stats {
        double minZ=Double.POSITIVE_INFINITY, maxZ=Double.NEGATIVE_INFINITY;
        List<Double> zs=new ArrayList<>();
    }

    private static class PCA {
        final double cx,cy;           // 质心
        final double ev1x, ev1y;      // 第一主方向单位向量（车斗长度方向）
        PCA(double cx,double cy,double ev1x,double ev1y){
            this.cx=cx; this.cy=cy; this.ev1x=ev1x; this.ev1y=ev1y;
        }
        static PCA onXY(List<double[]> pts){
            int n=pts.size();
            double mx=0,my=0; for (double[] p: pts){ mx+=p[0]; my+=p[1]; }
            mx/=n; my/=n;
            double sxx=0,sxy=0,syy=0;
            for (double[] p: pts){ double x=p[0]-mx, y=p[1]-my; sxx+=x*x; sxy+=x*y; syy+=y*y; }
            double theta = 0.5 * Math.atan2(2*sxy, (sxx - syy));
            return new PCA(mx,my, Math.cos(theta), Math.sin(theta));
        }
    }

    private static class OBB {
        final double uMin,uMax, vc;
        OBB(double uMin,double uMax,double vc){ this.uMin=uMin; this.uMax=uMax; this.vc=vc; }
        static OBB fromPoints(List<double[]> pts, PCA p){
            double uMin= Double.POSITIVE_INFINITY, uMax= Double.NEGATIVE_INFINITY;
            double vMin= Double.POSITIVE_INFINITY, vMax= Double.NEGATIVE_INFINITY;
            for (double[] pt: pts){
                double dx=pt[0]-p.cx, dy=pt[1]-p.cy;
                double u = dx*p.ev1x + dy*p.ev1y;
                double v = dx*(-p.ev1y) + dy*( p.ev1x);
                if (u<uMin) uMin=u; if (u>uMax) uMax=u;
                if (v<vMin) vMin=v; if (v>vMax) vMax=v;
            }
            double vc = 0.5*(vMax + vMin);
            return new OBB(uMin,uMax,vc);
        }
    }

    private static double[] fitPlane(List<double[]> pts) {
        // 最小二乘 z = a x + b y + c
        double Sx=0,Sy=0,Sz=0,Sxx=0,Syy=0,Sxy=0,Sxz=0,Syz=0; int n=0;
        for (double[] p : pts) {
            double x=p[0], y=p[1], z=p[2];
            if (!isFinite(x)||!isFinite(y)||!isFinite(z)) continue;
            Sx+=x; Sy+=y; Sz+=z; Sxx+=x*x; Syy+=y*y; Sxy+=x*y; Sxz+=x*z; Syz+=y*z; n++;
        }
        double[][] A = {{Sxx,Sxy,Sx},{Sxy,Syy,Sy},{Sx,Sy,n}};
        double[]   B = {Sxz,Syz,Sz};
        return solve3(A,B);
    }

    private static double[] solve3(double[][] A, double[] b) {
        int n=3; double[][] a=new double[n][n+1];
        for(int i=0;i<n;i++){ System.arraycopy(A[i],0,a[i],0,n); a[i][n]=b[i]; }
        for(int i=0;i<n;i++){
            int piv=i; for(int r=i+1;r<n;r++) if (Math.abs(a[r][i])>Math.abs(a[piv][i])) piv=r;
            double[] t=a[i]; a[i]=a[piv]; a[piv]=t;
            double div=a[i][i]; if (Math.abs(div)<1e-12) return new double[]{0,0,median(b)};
            for(int j=i;j<=n;j++) a[i][j]/=div;
            for(int r=0;r<n;r++){ if (r==i) continue; double f=a[r][i]; for(int j=i;j<=n;j++) a[r][j]-=f*a[i][j]; }
        }
        return new double[]{a[0][3],a[1][3],a[2][3]};
    }

    private static double median(double[] v){ double[] c=v.clone(); Arrays.sort(c); int n=c.length; return n%2==1?c[n/2]:0.5*(c[n/2-1]+c[n/2]); }
    private static double medianZ(List<double[]> pts){
        double[] z=new double[pts.size()];
        for (int i=0;i<pts.size();i++) z[i]=pts.get(i)[2];
        Arrays.sort(z); int n=z.length; return (n%2==1)?z[n/2]:0.5*(z[n/2-1]+z[n/2]);
    }
}
